/*!
 * Copyright (c) 2012
 * Automation and Robotics Lab (LARA) at University of Brasilia (UnB)
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 *     - Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     - Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     - Neither the name of the Automation and Robotics Lab (LARA) nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License LGPL as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
 * GNU Lesser General Public License LGPL for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License LGPL along with this program.
 * If not, see <http://www.gnu.org/licenses/>.
 *
 *****************************************************************
 *
 *   ROS stack name: schunk
 *   ROS package name: schunk_low
 *
 *   Authors: André A. Geraldes, email: andregeraldes@lara.unb.br
 *            Murilo M. Marinho, email: murilomarinho@lara.unb.br
 *
 *****************************************************************/

/******************************************************************************
                                   INCLUDES
 *****************************************************************************/

// Schunk_low
#include <schunk_low/schunk.h>

// General
#include<string>
#include<vector>

// ROS
#include <ros/ros.h>

// Library for communicating with the hardware
#include <schunk_libm5api/m5apiw32.h>

// Namespace std
using namespace std;

/******************************************************************************
             METHODS FOR STORING ROBOT'S INFORMATION IN MESSAGES
 *****************************************************************************/

schunk_msgs::JointStateComplete Schunk::ReadJointStates()
{
    float float_data;
    long long_data;
    short short_data;
    unsigned long ulong_data;
    unsigned char uchar_data;

    for (int i = 0; i < module_count_; i++)
    {
        pthread_mutex_lock(&mutex_);
        PCube_getModuleState(device_, module_ids_[i], &ulong_data);
        joint_msg_.ModuleState[i] = static_cast<int>(ulong_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getStateDioPos(device_, module_ids_[i], &ulong_data, &uchar_data, &float_data);
        joint_msg_.Dio[i] = uchar_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getPos(device_, module_ids_[i], &float_data);
        joint_msg_.Pos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getPosInc(device_, module_ids_[i], &long_data);
        joint_msg_.PosInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getPosCountInc(device_, module_ids_[i], &long_data);
        joint_msg_.PosCountInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getVel(device_, module_ids_[i], &float_data);
        joint_msg_.Vel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getVelInc(device_, module_ids_[i], &long_data);
        joint_msg_.VelInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getIPolVel(device_, module_ids_[i], &float_data);
        joint_msg_.IPolVel[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getCur(device_, module_ids_[i], &float_data);
        joint_msg_.Cur[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getCurInc(device_, module_ids_[i], &short_data);
        joint_msg_.CurInc[i] = short_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDeltaPos(device_, module_ids_[i], &float_data);
        joint_msg_.DeltaPos[i] = float_data;
        pthread_mutex_unlock(&mutex_);

        pthread_mutex_lock(&mutex_);
        PCube_getDeltaPosInc(device_, module_ids_[i], &long_data);
        joint_msg_.DeltaPosInc[i] = static_cast<int>(long_data);
        pthread_mutex_unlock(&mutex_);
    }

    return joint_msg_;
}
